﻿ // SatelliteNavigation_pcx.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。

#include <iostream>
#include <string.h>
//#include <Windows.h>
//#include "SimpleIni.h"
#include "Struct.h"
#include "eigen-3.4.0/Eigen/Eigen"

#pragma comment(lib,"WS2_32.lib")
#pragma warning(disable:4996)
using namespace std;
int main()
{
	RAWDATA RAW,RAW_back;
	EPHREC GpsEph[MAXGPSNUM], BdsEph[MAXBDSNUM];//定义全局变量，不随历元传递

	int isSyn = 0;/*是否同步成功 0需要继续同步 1同步成功 -1文件结束*/
	int isFloat = 0;/*浮点解是否计算成功 0 成功*/
	int isFix = 0;

	RTKEKF kf;
	ROVERCFGINFO cfg;
	FILE *bfp, *rfp;
	FILE* out;
	SOCKET BNetGps, RNetGps;
	GEOCOOR BLHPos;

	/*打开输出文件*/
	if ((out = fopen(cfg.ResFile.c_str(), "w+")) == NULL)
	{
		printf("Cannot open out file\n");
		return -2;
	}

	/*打开配置文件*/
	if (!ReadRTKConfigInfo("config.ini", cfg))return -1;

	/*开始同步*/
	if (cfg.IsFileData == 1)//文件同步
	{
		if ((bfp = fopen(cfg.BasObsDatFile.c_str(), "rb")) == NULL)
		{
			printf("Cannot open Baseobs file\n");
			return -2;
		}
		if ((rfp = fopen(cfg.RovObsDatFile.c_str(), "rb")) == NULL)
		{
			printf("Cannot open Rovobs file\n");
			return -2;
		}

		while (true)
		{
			isSyn = GetSynObsFile(bfp, rfp, RAW, GpsEph, BdsEph);

			//if (RAW.SdObs.Time.SecOfWeek+1E-3 >= 284278.000)
			//	cout << " ";

			if (isSyn == 0)continue;
			else if (isSyn == -1)break;
			else if (isSyn == 1) 
			{
				///*printf("Base and Data Syn ok!\n");*/
				//if (RAW.RovObs.Time.SecOfWeek < 284208.000)continue;

				/*观测值粗差探测*/
				DetectOutlier(&RAW.BaseObs);
				DetectOutlier(&RAW.RovObs);

				/*SPP单点定位*/
				if (SPP(&RAW.BaseObs, GpsEph, BdsEph) == 0)SPV(&RAW.BaseObs);
				if (SPP(&RAW.RovObs, GpsEph, BdsEph) == 0)SPV(&RAW.RovObs);

				/*解算单差观测值*/
				FormSDEpochObs(&RAW.BaseObs,&RAW.RovObs,&RAW.SdObs);
				DetectCycleSlip(&RAW.BaseObs, &RAW.RovObs, &RAW_back.BaseObs, &RAW_back.RovObs, &RAW.SdObs);

				/*选取参考星*/
				DetRefSat(&RAW.BaseObs, &RAW.RovObs, &RAW.SdObs, &RAW.DDObs);

				/*模糊度浮点解解算*/
				if (cfg.RTKMode == 0)isFloat = RTKFloat(&RAW, &RAW.BaseObs.RcvPos, &RAW.RovObs.SppPos);//LSQ
				else if (cfg.RTKMode == 1)isFloat = RTK_KF(&RAW, &RAW_back, &kf);

				if (isFloat == 0)
				{
					isFix = 0;//不成功
					if (lambda(RAW.DDObs.DDSatNum[0] + RAW.DDObs.DDSatNum[1], 2, RAW.DDObs.FloatAmb,
						RAW.DDObs.Qnn, RAW.DDObs.FixedAmb, RAW.DDObs.ResAmb) == 0)
					{
						RAW.DDObs.Ratio = RAW.DDObs.ResAmb[1] / RAW.DDObs.ResAmb[0];
						if (RAW.DDObs.Ratio > MAXRATIO)
						{
							RTKFixed(RAW.DDObs.DDSatNum[0] + RAW.DDObs.DDSatNum[1], 2, RAW.DDObs.FloatAmb, RAW.DDObs.FixedAmb,
								RAW.DDObs.Qbn, RAW.DDObs.Qnn, &RAW.DDObs.FloatPos, RAW.DDObs.FixPos);

							RAW.DDObs.isFixed = true;
							isFix = 1;

							XYZtoBLH(RAW.DDObs.FixPos[0], BLHPos, WGS84);
							/*printf("%d %.3f %.5f %.5f  %.5f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FixPos[0].Xyz[0], RAW.DDObs.FixPos[0].Xyz[1], RAW.DDObs.FixPos[0].Xyz[2], RAW.RovObs.RcvPos.XYZPos.Xyz[0], RAW.RovObs.RcvPos.XYZPos.Xyz[1], RAW.RovObs.RcvPos.XYZPos.Xyz[2], RAW.DDObs.Ratio, isFix);*/
							fprintf(out, "%d %.3f %.5f %.5f %.5f %.3f %.3f %.3f %.3f %d %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FixPos[0].Xyz[0], RAW.DDObs.FixPos[0].Xyz[1], RAW.DDObs.FixPos[0].Xyz[2], RAW.DDObs.Ratio, RAW.DDObs.Sats, isFix);
							fflush(out);
							printf("%d %.3f %.5f %.5f %.5f %.3f %.3f %.3f %.3f %d %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FixPos[0].Xyz[0], RAW.DDObs.FixPos[0].Xyz[1], RAW.DDObs.FixPos[0].Xyz[2], RAW.DDObs.Ratio, RAW.DDObs.Sats, isFix);
							/*printf("RTK Fixed OK!\n");*/
						}
					}

					if (isFix == 0)
					{
						XYZtoBLH(RAW.DDObs.FloatPos, BLHPos, WGS84);
						/*printf("%d %.3f %.5f %.5f  %.f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FloatPos.Xyz[0], RAW.DDObs.FloatPos.Xyz[1], RAW.DDObs.FloatPos.Xyz[2], RAW.RovObs.RcvPos.XYZPos.Xyz[0], RAW.RovObs.RcvPos.XYZPos.Xyz[1], RAW.RovObs.RcvPos.XYZPos.Xyz[2], RAW.DDObs.Ratio, isFix);*/
						fprintf(out, "%d %.3f %.5f %.5f %.5f %.3f %.3f %.3f %.3f %d %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FloatPos.Xyz[0], RAW.DDObs.FloatPos.Xyz[1], RAW.DDObs.FloatPos.Xyz[2], RAW.DDObs.Ratio, RAW.DDObs.Sats, isFix);
						fflush(out);
						printf("%d %.3f %.5f %.5f %.5f %.3f %.3f %.3f %.3f %d %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FloatPos.Xyz[0], RAW.DDObs.FloatPos.Xyz[1], RAW.DDObs.FloatPos.Xyz[2], RAW.DDObs.Ratio, RAW.DDObs.Sats, isFix);
					}
				}


				/*保存上个历元的数据*/
		/*		memcpy(&RAW_back, &RAW, sizeof(RAW));*/
				RAW_back.BaseObs = RAW.BaseObs;
				RAW_back.RovObs = RAW.RovObs;
				RAW_back.SdObs = RAW.SdObs;
				RAW_back.DDObs = RAW.DDObs;
				continue;
			}
		}

		fclose(bfp);
		fclose(rfp);
		fclose(out);

		return 0;
	}
	if (cfg.IsFileData == 0)//串口同步
	{
		if (OpenSocket(BNetGps, (const char*)cfg.BasNetIP.c_str()/*"47.114.134.129"*/, cfg.BasNetPort) == false)
		{
			printf("This Base ip & port was not opened.\n");
			return -2;
		}
		if (OpenSocket(RNetGps, (const char*)cfg.RovNetIP.c_str(),/*"47.114.134.129",*/cfg.RovNetPort) == false)
		{
			printf("This Rov ip & port was not opened.\n");
			return -2;
		}

		while (true)
		{
			isSyn = GetSynObsCOM(BNetGps, RNetGps, RAW,GpsEph,BdsEph);

			if (isSyn == 0)continue;
			else if (isSyn == -1)break;
			else if (isSyn == 1)
			{
				///*printf("Base and Data Syn ok!\n");*/
				//if (RAW.RovObs.Time.SecOfWeek < 284208.000)continue;

				/*观测值粗差探测*/
				DetectOutlier(&RAW.BaseObs);
				DetectOutlier(&RAW.RovObs);

				/*SPP单点定位*/
				if (SPP(&RAW.BaseObs, GpsEph, BdsEph) == 0)SPV(&RAW.BaseObs);
				if (SPP(&RAW.RovObs, GpsEph, BdsEph) == 0)SPV(&RAW.RovObs);

				/*解算单差观测值*/
				FormSDEpochObs(&RAW.BaseObs, &RAW.RovObs, &RAW.SdObs);
				DetectCycleSlip(&RAW.BaseObs, &RAW.RovObs, &RAW_back.BaseObs, &RAW_back.RovObs, &RAW.SdObs);

				/*选取参考星*/
				DetRefSat(&RAW.BaseObs, &RAW.RovObs, &RAW.SdObs, &RAW.DDObs);

				/*模糊度浮点解解算*/
				if (cfg.RTKMode == 0)isFloat = RTKFloat(&RAW, &RAW.BaseObs.RcvPos, &RAW.RovObs.SppPos);//LSQ
				else if (cfg.RTKMode == 1)isFloat = RTK_KF(&RAW, &RAW_back, &kf);

				if (isFloat == 0)
				{
					isFix = 0;//不成功
					if (lambda(RAW.DDObs.DDSatNum[0] + RAW.DDObs.DDSatNum[1], 2, RAW.DDObs.FloatAmb,
						RAW.DDObs.Qnn, RAW.DDObs.FixedAmb, RAW.DDObs.ResAmb) == 0)
					{
						RAW.DDObs.Ratio = RAW.DDObs.ResAmb[1] / RAW.DDObs.ResAmb[0];
						if (RAW.DDObs.Ratio > MAXRATIO)
						{
							RTKFixed(RAW.DDObs.DDSatNum[0] + RAW.DDObs.DDSatNum[1], 2, RAW.DDObs.FloatAmb, RAW.DDObs.FixedAmb,
								RAW.DDObs.Qbn, RAW.DDObs.Qnn, &RAW.DDObs.FloatPos, RAW.DDObs.FixPos);

							RAW.DDObs.isFixed = true;
							isFix = 1;

							XYZtoBLH(RAW.DDObs.FixPos[0], BLHPos, WGS84);
							/*printf("%d %.3f %.5f %.5f  %.5f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FixPos[0].Xyz[0], RAW.DDObs.FixPos[0].Xyz[1], RAW.DDObs.FixPos[0].Xyz[2], RAW.RovObs.RcvPos.XYZPos.Xyz[0], RAW.RovObs.RcvPos.XYZPos.Xyz[1], RAW.RovObs.RcvPos.XYZPos.Xyz[2], RAW.DDObs.Ratio, isFix);*/
							fprintf(out,"%d %.3f %.5f %.5f %.5f %.3f %.3f %.3f %.3f %d %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FixPos[0].Xyz[0], RAW.DDObs.FixPos[0].Xyz[1], RAW.DDObs.FixPos[0].Xyz[2], RAW.DDObs.Ratio, RAW.DDObs.Sats, isFix);
							fflush(out);
							printf("%d %.3f %.5f %.5f %.5f %.3f %.3f %.3f %.3f %d %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FixPos[0].Xyz[0], RAW.DDObs.FixPos[0].Xyz[1], RAW.DDObs.FixPos[0].Xyz[2], RAW.DDObs.Ratio, RAW.DDObs.Sats, isFix);
							/*printf("RTK Fixed OK!\n");*/
						}
					}

					if (isFix == 0)
					{
						XYZtoBLH(RAW.DDObs.FloatPos, BLHPos, WGS84);
						/*printf("%d %.3f %.5f %.5f  %.f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FloatPos.Xyz[0], RAW.DDObs.FloatPos.Xyz[1], RAW.DDObs.FloatPos.Xyz[2], RAW.RovObs.RcvPos.XYZPos.Xyz[0], RAW.RovObs.RcvPos.XYZPos.Xyz[1], RAW.RovObs.RcvPos.XYZPos.Xyz[2], RAW.DDObs.Ratio, isFix);*/
						fprintf(out,"%d %.3f %.5f %.5f %.5f %.3f %.3f %.3f %.3f %d %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FloatPos.Xyz[0], RAW.DDObs.FloatPos.Xyz[1], RAW.DDObs.FloatPos.Xyz[2], RAW.DDObs.Ratio, RAW.DDObs.Sats ,isFix); 
						fflush(out);
						printf("%d %.3f %.5f %.5f %.5f %.3f %.3f %.3f %.3f %d %d\n", RAW.RovObs.Time.Week, RAW.RovObs.Time.SecOfWeek, BLHPos.Blh[0] * Deg, BLHPos.Blh[1] * Deg, BLHPos.Blh[2], RAW.DDObs.FloatPos.Xyz[0], RAW.DDObs.FloatPos.Xyz[1], RAW.DDObs.FloatPos.Xyz[2], RAW.DDObs.Ratio, RAW.DDObs.Sats, isFix);
					}
				}

				/*保存上个历元的数据*/
		/*		memcpy(&RAW_back, &RAW, sizeof(RAW));*/
				RAW_back.BaseObs = RAW.BaseObs;
				RAW_back.RovObs = RAW.RovObs;
				RAW_back.SdObs = RAW.SdObs;
				RAW_back.DDObs = RAW.DDObs;
				continue;
			}
		}

		CloseSocket(BNetGps);
		CloseSocket(RNetGps);
		fclose(out);
		return 0;
	}

}


//int main()
//{
//	EPOCHOBS obs;
//	EPHREC beph[MAXBDSNUM], geph[MAXGPSNUM];
//	CARCOOR UserCOOR;
//	CARCOOR RefCOOR = { -2267804.5263,5009342.3723,3220991.8632 };
//	ENUCOOR enu;
//	int OpenWay;
//
//	//打开文件
//	unsigned char buff[MAXRAWLEN] = {};
//	
//	int LenRead;  /*读取总长度*/
//	int d=0; /*上一次残余长度*/
//	int isOBS;
//	FILE* fp;
//
//	printf("please choose a open-way: 1.file 2.socket:");
//	cin >> OpenWay;
//
//	if (OpenWay == 1)
//	{
//		if ((fp = fopen("oem719-202404021900-1.bin", "rb")) == NULL)
//		{
//			printf("Cannot open obs file\n");
//			return 0;
//		}
//
//		FILE *outfile = fopen("PosData.txt","w+");
//		if (outfile == NULL)
//		{
//			printf("Cannot write it in file\n");
//			return 0;
//		}
//
//		fprintf(outfile, "Wk Sow B(deg) L(deg) H(m) X Y Z E N U SigmaX SigmaY SigmaZ PDOP UserSatNum\n");
//
//		while (!feof(fp))
//		{
//			LenRead = fread(buff + d, sizeof(unsigned char), MAXRAWLEN - d, fp);
//			/*每次解算开始时重新计算长度*/
//			d = d + LenRead;
//			isOBS=DecodeNovOem7Dat(buff, d, &obs, geph, beph);
//
//			if (isOBS == 1)/*开始进行卫星SPP解算*/
//			{
//				/*粗差探测 标记SatOBS*/
//				DetectOutlier(&obs);
//
//				/*SPP解算*/
//				if (SPP(&obs, geph, beph) == 0)
//				{
//					SPV(&obs);
//					BLHtoXYZ(obs.SppPos.Pos, UserCOOR, WGS84);
//					CompEnudPos(UserCOOR, RefCOOR, enu, WGS84);
//					fprintf(outfile, "%4d %10.4f %13.4f %13.4f %13.4f %13.4f %13.4f %13.4f %13.4f %13.4f %13.4f %13.4f %13.4f %13.4f %13.4f %2d\n", obs.SppPos.Time.Week, obs.SppPos.Time.SecOfWeek, obs.SppPos.Pos.Blh[0] * Deg, obs.SppPos.Pos.Blh[1] * Deg, obs.SppPos.Pos.Blh[2], UserCOOR.XYZ.x, UserCOOR.XYZ.y, UserCOOR.XYZ.z, enu.ENU.dE, enu.ENU.dN, enu.ENU.dU, obs.SppPos.SigmaPos[0], obs.SppPos.SigmaPos[1], obs.SppPos.SigmaPos[2], obs.SppPos.PDOP, obs.SppPos.UseSatNum);
//				}
//			}
//		}
//		fclose(fp);
//		fclose(outfile);
//	}
//
//	if (OpenWay == 2)
//	{
//		SOCKET NetGps;
//
//		if (OpenSocket(NetGps, "47.114.134.129", 7190) == false)
//		{
//			printf("This ip & port was not opened.\n");
//			return 0;
//		}
//
//		do
//		{
//			Sleep(980);
//			if ((LenRead = recv(NetGps, (char*)buff, MAXRAWLEN, 0)) > 0) memcpy(buff + d, buff, LenRead);
//
//			/*每次解算开始时重新计算长度*/
//			d = d + LenRead;
//			isOBS=DecodeNovOem7Dat(buff, d, &obs, geph, beph);
//
//
//			if (isOBS == 1)/*开始进行卫星SPP解算*/
//			{
//				/*粗差探测 标记SatOBS*/
//				DetectOutlier(&obs);
//
//				/*SPP解算*/
//				if (SPP(&obs, geph, beph) == 0)
//				{
//					SPV(&obs);
//					BLHtoXYZ(obs.SppPos.Pos, UserCOOR, WGS84);
//					printf("%4d %10.3f %10.3f %10.3f %10.2f %10.3f %10.3f %10.3f %2d\n", obs.SppPos.Time.Week, obs.SppPos.Time.SecOfWeek, obs.SppPos.Pos.Blh[0] * Deg, obs.SppPos.Pos.Blh[1] * Deg, obs.SppPos.Pos.Blh[2], UserCOOR.XYZ.x, UserCOOR.XYZ.y, UserCOOR.XYZ.z, obs.SppPos.UseSatNum);
//				}
//			}
//
//		} while (d >= 0);
//
//		CloseSocket(NetGps);
//	}
//
//
//	return 0;
//}


// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门使用技巧: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
